# coding=UTF-8

import numpy as np
import cv2
import matchfunc
from pointTracks import *


class viewSet:
    def __init__(self):
        ##list中列表数量必须保持一致，一致的增加或减少
        self.idx_list=[]            ##相机的索引
        self.orient_list=[]         ##相机在参考坐标系中的绝对朝向
        self.loc_list=[]            ##相机在参考坐标系中的绝对位置
        self.kps_list=[]            ##相机中较好的关键点，每个相机对应一个列表
        self.desc_list=[]           ##相机中较好的关键点对应的描述符，每个相机对应一个列表
        self.match_dict={}          ##(idx1,id2x)-->matches   当前视图idx2与前一个视图idx1之间的点匹配
        return

    def addView(self,idx,absOrient,absLoc,keyPoints,descriptors,match_pair):
        """
        功能：为视图集添加一个视图
        输入：
            1,idx-->视图的索引
            2,absOrient-->该视图在参考坐标系中的绝对朝向
            3,absLoc-->该视图在参考坐标系中的绝对位置
            4,keyPoints-->该视图检测到的所有关键点
            5,matches-->该视图与前一个视图之间的点匹配
        输出：
            无
        """
        ##检查是否存在相同的ID
        if idx in self.idx_list:
            return False
        self.idx_list.append(idx)
        self.orient_list.append(absOrient)
        self.loc_list.append(absLoc)
        self.kps_list.append(keyPoints)
        self.desc_list.append(descriptors)
        self.match_dict[(match_pair[0],idx)]=match_pair[1]
        return True

    def getPoses(self,idx):
        """
        功能：获得指定idx的view的参数,包括absOrient，absLoc，keypoints
        """
        if idx not in self.idx_list:
            return None,None
        pos=self.idx_list.index(idx)   #找到在对应列表中的索引
        return self.orient_list[pos],self.loc_list[pos]

    def getAllPoses(self):
        """
        功能：获取所有的视图的相机方位
        输出：
            相机字典，每个元素对应一个相机,[idx, idx,np.array([.orient.]),np.array([[x],[y],[z]])]
        """
        poses_dict=[]
        for i in range(len(self.idx_list)):
            poses_dict.append([i,self.orient_list[i], self.loc_list[i] ])
        return poses_dict

    def getFeatures(self,idx):
        """
        功能：获得指定id的view的特征,包括keypoints，descriptors
        """
        if id not in self.idx_list:
            return None,None
        pos=self.idx_list.index(idx)   #找到在对应列表中的索引
        return self.kps_list[pos],self.desc_list[pos]


    def getTracks(self,trackList,idx,pts):
        """
        功能：在trackList中查找包含[id,(x,y)]的PointTracks并返回
        输入：
            pts=np.array([x,y])
        返回：

        """
        for track in trackList:
            if idx in track.idx_list:
                i=track.idx_list.index(idx)
                if track.pos_list[i]==(pts[0],pts[1]):
                    return track
        return None

    def generateTracks(self):
        """
        功能：获取所有视图的共同匹配点，及它们按图像序列的轨迹点
        输出：
            tracks-->PointTracks对象的列表
        """
        track_list=[]       #保存创建的PointTrack
        M=len(self.idx_list)
        for i in range(1,M):
            ##取(i,j)的点匹配列表
            prevIdx=self.idx_list[i-1]
            curIdx=self.idx_list[i]
            matches = self.match_dict[(prevIdx, curIdx)]
            prevPts, curPts = matchfunc.fetchCoordinate(self.kps_list[i-1], self.kps_list[i],matches)
            N=len(prevPts)
            for n in range(N):  ##处理每一个匹配点
                ##检查是否已经创建了对应的PointTrack了，有则加入，没有则重新创建
                track=self.getTracks(track_list,prevIdx,prevPts[n])
                if track==None:
                    track=PointTrack()
                    track.addMatch(prevIdx,prevPts[n])
                track.addMatch(curIdx,curPts[n])
                track_list.append(track)
        return track_list
